//============================================================================
// Name        : cvk-calib.cpp
// Author      : Maurilio Di Cicco
// Version     : 0.1
// Copyright   : nah.
// Description : OpenCV Kinect color based calibration tool
//============================================================================

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include "opencv2/opencv.hpp"
#include "libconfig.h++"
extern "C" {
#include "linenoise.h"
}



using namespace std;
using namespace cv;
using namespace libconfig;


//GLOBALS
//============================================================================
Mat depthMap;
Mat bgrImage;
Mat hsvImage;
vector<Vec3b> clicks;
vector<Vec3i> rangesLOW;
vector<Vec3i> rangesHIGH;

int erosion_elem = 0;
int erosion_size = 0;

int zmin=0;
int zmax=5000;

int status=0;
int blob=0;
int calibration=0;
Config cfg;
char* finger;
char* color;

//============================================================================



//CALLBACKS
//============================================================================
static void onClickCFG(int state, void* userdata){
	char buff[50];
	char names[9][50];

	sprintf(names[0],"%s","hmin");
	sprintf(names[1],"%s","hmax");
	sprintf(names[2],"%s","smin");
	sprintf(names[3],"%s","smax");
	sprintf(names[4],"%s","vmin");
	sprintf(names[5],"%s","vmax");
	sprintf(names[6],"%s","erodeElem");
	sprintf(names[7],"%s","erodeElem");
	sprintf(names[8],"%s","color");


	sprintf (buff, "%s.%s",finger,names[0]);
	Setting& cfg_hmin = cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[1]);
	Setting& cfg_hmax=cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[2]);
	Setting& cfg_smin= cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[3]);
	Setting& cfg_smax= cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[4]);
	Setting& cfg_vmin= cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[5]);
	Setting& cfg_vmax= cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[6]);
	Setting& cfg_erodeElem= cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[7]);
	Setting& cfg_erodeSize= cfg.lookup(buff);

	sprintf (buff, "%s.%s",finger,names[8]);
	Setting& cfg_color= cfg.lookup(buff);


	cfg_hmin=rangesLOW[0][0];
	cfg_smin=rangesLOW[0][1];
	cfg_vmin=rangesLOW[0][2];

	cfg_hmax=rangesHIGH[0][0];
	cfg_smax=rangesHIGH[0][1];
	cfg_vmax=rangesHIGH[0][2];

	cfg_erodeElem=erosion_elem;
	cfg_erodeSize=erosion_size;

	cfg_color=color;

	cfg.writeFile("color.cfg");
	calibration=0;
}

static void onClickBLOB(int state, void* userdata){
	if(blob==0)
	{
		blob=1;
	}
	else
	{
		blob=0;
	}



}

static void clickRGB(int event, int x, int y, int, void*)
{
	if( event != CV_EVENT_LBUTTONDOWN ) return;


	Vec3b colors = bgrImage.at<Vec3b>(y, x);

	int blue = (int)colors.val[0];
	int green = (int)colors.val[1];
	int red = (int)colors.val[2];

	//clicks.push_back(colors);
	//rangesLOW.push_back((Vec3i)colors);
	//rangesHIGH.push_back((Vec3i)colors);

	cout<<"click at x: "<<x<<" y: "<< y <<" HSV: ("<<blue<<","<<green<<","<<red<<")"<<endl;
	cout<<"click at x: "<<x<<" y: "<< y <<" HSV: ("<<rangesLOW[0][0]<<","<<rangesLOW[0][1]<<","<<rangesLOW[0][2]<<")"<<endl;
	status=1;

}

//============================================================================



int main() {


	//LIBCONFIG

	cfg.readFile("color.cfg");



	VideoCapture capture(CV_CAP_OPENNI);
	namedWindow("RGB",CV_WINDOW_AUTOSIZE);
	createTrackbar( "Z min", "RGB", &zmin, 5000,  NULL);
	createTrackbar( "Z max", "RGB", &zmax, 5000,  NULL);

	setMouseCallback("RGB",clickRGB);

	char winName[60];
	sprintf (winName, "Filtering %d", 1);
	namedWindow(winName);

	Vec3b colors;
	colors[0]=colors[1]=colors[2]=0;
	rangesLOW.push_back((Vec3i)colors);
	rangesHIGH.push_back((Vec3i)colors);
	erosion_elem=0;
	erosion_size=0;

	createTrackbar( "H min", winName, &rangesLOW[0][0], 255,  NULL);
	createTrackbar( "H max", winName, &rangesHIGH[0][0], 255,  NULL);

	createTrackbar( "S min", winName, &rangesLOW[0][1], 255,  NULL);
	createTrackbar( "S max", winName, &rangesHIGH[0][1], 255,  NULL);

	createTrackbar( "V min", winName, &rangesLOW[0][2], 255,  NULL);
	createTrackbar( "V max", winName, &rangesHIGH[0][2], 255,  NULL);

	createTrackbar( "Erosion Elem", winName, &erosion_elem, 2,  NULL);
	createTrackbar( "Erosion Size", winName, &erosion_size, 10,  NULL);

	createButton("Start/Stop BLOB process", onClickBLOB);
	createButton("Save to conf", onClickCFG);

	//COMANDI DA TERMINALE
	char* line;
	char* quit="quit";
	char* help="help";
	char* calibrate="calibrate";

	printf("Write 'help' to get some help\n");

	while((line = linenoise("calibration tool>> ")) != NULL){
		if(*line==*quit){
			return 1;
		}
		if(*line==*help){
			printf("\nCommands:\n");
			printf("quit - quit the program\n");
			printf("help - calls batman\n");
			printf("fingers - thumb index middle ring pinky \n");
			printf("calibrate FINGER COLOR - run calibration routine for the FINGER finger with the COLOR color\n\n");
		}

		if(*line==*calibrate)
		{
			finger=strtok(line," ");
			finger=strtok(NULL," ");
			color=strtok(NULL," ");
			char test[50];
			sprintf(test,"%s.hmin",finger);
			int testValue=0;
			if(cfg.lookupValue(test,testValue)==0)
			{
				printf("Finger not exists, call 'help' for help\n");

			}
			else{


			printf("Running calibration routine for [%s] finger with color [%s] \n",finger,color);
			calibration=1;
			}
			while(calibration)
			{


				capture.grab();
				capture.retrieve( depthMap, CV_CAP_OPENNI_DEPTH_MAP );
				capture.retrieve( bgrImage, CV_CAP_OPENNI_BGR_IMAGE );

				int cols=depthMap.cols;
				int rows=depthMap.rows;

				for(int i=0;i<rows;i++)
				{
					for(int j=0;j<cols;j++)
					{
						if(depthMap.at<short>(j+i*cols)>zmax || depthMap.at<short>(j+i*cols)<zmin)
						{
							bgrImage.at<Vec3b>(j+i*cols)[0]=0;
							bgrImage.at<Vec3b>(j+i*cols)[1]=0;
							bgrImage.at<Vec3b>(j+i*cols)[2]=0;
						}
					}
				}



				if(status==1)
				{
					cvtColor(bgrImage,hsvImage, CV_BGR2HSV);
					inRange(hsvImage,Scalar(rangesLOW[0][0],rangesLOW[0][1],rangesLOW[0][2]),
							Scalar(rangesHIGH[0][0],rangesHIGH[0][1],rangesHIGH[0][2]),hsvImage);

					int erosion_type;
					if( erosion_elem == 0 ){ erosion_type = MORPH_RECT; }
					else if( erosion_elem == 1 ){ erosion_type = MORPH_CROSS; }
					else if( erosion_elem == 2) { erosion_type = MORPH_ELLIPSE; }


					Mat element = getStructuringElement( erosion_type,
							Size( 2*erosion_size + 1, 2*erosion_size+1 ),
							Point( erosion_size, erosion_size ) );

					erode( hsvImage, hsvImage, element );

					imshow(winName,hsvImage);

					SimpleBlobDetector::Params parametri;
					parametri.filterByArea=1;
					parametri.maxArea=10000;
					parametri.minArea=2;

					if(blob){
						SimpleBlobDetector *blobDetector=new SimpleBlobDetector(parametri);
						blobDetector->create("SimpleBlobDetector");
						vector<KeyPoint> punti;
						blobDetector->detect(hsvImage,punti);

						Point2i mediaBlob;
						mediaBlob.x=0;
						mediaBlob.y=0;

						int size=(int)punti.size();

						if(size!=0)
						{
							for(int i=0;i<size;i++)
							{
								mediaBlob.x+=punti[i].pt.x;
								mediaBlob.y+=punti[i].pt.y;
							}
							mediaBlob.x=(int)(mediaBlob.x/size);
							mediaBlob.y=(int)(mediaBlob.y/size);
							circle(bgrImage,mediaBlob,10,Scalar(0,0,255));
						}
					}

				}




				imshow("RGB",bgrImage);
				waitKey(33);
			}
		}
	}
	cvDestroyAllWindows();
	return 0;
}
